// Copyright (c) 2024，D-Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#include <chrono>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include "hobotcv_imgproc/hobotcv_front.h"
#include "hobotcv_imgproc/hobotcv_imgproc.h"
#include "rclcpp/rclcpp.hpp"
#include "utils.h"

namespace hobot_cv {

#define ERR_CON_EQ(ret, a) do {\
		if ((ret) != (a)) {\
			printf("%s(%d) failed, ret %d\n", __func__, __LINE__, (int32_t)(ret));\
			return (ret);\
		}\
	} while(0)\


#define ERR_CON_NE(ret, a) do {\
		if ((ret) == (a)) {\
			printf("%s(%d) failed, ret %ld\n", __func__, __LINE__, (ret));\
			return (ret);\
		}\
	} while(0)\


bool check_padding_area(uint32_t top,
                        uint32_t bottom,
                        uint32_t left,
                        uint32_t right,
                        const int &src_h,
                        const int &src_w,
                        int padding_type) {
  if (padding_type == (int)(HobotcvPaddingType::HOBOTCV_REFLECT)) {
    // HOBOTCV_REFLECT方式，padding尺寸如果超过原图尺寸会发生越界
    if ((int)top > src_h) {
      RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"),
                   "Invalid top size: %d! src_h: %d , padding top must be less "
                   "than src height!",
                   top,
                   src_h);
      return false;
    } else if ((int)bottom > src_h) {
      RCLCPP_ERROR(
          rclcpp::get_logger("hobot_cv padding"),
          "Invalid bottom size: %d! src_h: %d , padding bottom must be less "
          "than src height!",
          bottom,
          src_h);
      return false;
    } else if ((int)left > src_w) {
      RCLCPP_ERROR(
          rclcpp::get_logger("hobot_cv padding"),
          "Invalid left size: %d! src_w: %d , padding left must be less "
          "than src width!",
          left,
          src_w);
      return false;
    } else if ((int)right > src_w) {
      RCLCPP_ERROR(
          rclcpp::get_logger("hobot_cv padding"),
          "Invalid right size: %d! src_w: %d , padding right must be less "
          "than src width!",
          right,
          src_w);
      return false;
    }
  }
  if (top == 0 && bottom == 0 && left == 0 && right == 0) {
    RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"), "No padding area!");
    return false;
  } else if (top % 2 != 0) {
    RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"),
                 "Invalid top size: %d! Padding size must be even",
                 top);
    return false;
  } else if (bottom % 2 != 0) {
    RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"),
                 "Invalid bottom size: %d! Padding size must be even",
                 bottom);
    return false;
  } else if (left % 2 != 0) {
    RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"),
                 "Invalid left size: %d! Padding size must be even",
                 left);
    return false;
  } else if (right % 2 != 0) {
    RCLCPP_ERROR(rclcpp::get_logger("hobot_cv padding"),
                 "Invalid right size: %d! Padding size must be even",
                 right);
    return false;
  }
  return true;
}

std::unique_ptr<char[]> hobotcv_constant_padding(const char *src,
                                                 const int &src_h,
                                                 const int &src_w,
                                                 uint32_t top,
                                                 uint32_t bottom,
                                                 uint32_t left,
                                                 uint32_t right,
                                                 uint8_t value) {
  // value转成yuv值
  uint8_t value_y = value;
  uint8_t value_u = 128;
  uint8_t value_v = 128;

  uint32_t dst_w = src_w + left + right;
  uint32_t dst_h = src_h + top + bottom;
  uint32_t dst_y_size = dst_w * dst_h;
  uint32_t dst_uv_size = dst_w * dst_h / 2;
  size_t dst_size = dst_y_size + dst_uv_size;
  std::unique_ptr<char[]> unique(new char[dst_size]);
  auto dst = unique.get();
  char *dst_y = dst;
  char *dst_uv = dst + dst_y_size;

  // padding y
  for (uint32_t h = 0; h < dst_h; ++h) {
    if (h < top || h >= src_h + top) {
      auto *raw = dst_y + h * dst_w;
      memset(raw, value_y, dst_w);
    } else {
      // padding left
      auto *raw = dst_y + h * dst_w;
      memset(raw, value_y, left);
      // copy src
      raw = dst_y + h * dst_w + left;
      auto *src_y_raw = src + (h - top) * src_w;
      memcpy(raw, src_y_raw, src_w);
      // padding right
      raw = dst_y + h * dst_w + left + src_w;
      memset(raw, value_y, right);
    }
  }

  // padding uv
  auto *src_uv_data = src + src_h * src_w;
  for (uint32_t h = 0; h < dst_h / 2; ++h) {
    auto *raw = dst_uv + h * dst_w;
    if ((h < (top / 2)) || (h >= (src_h + top) / 2)) {
      for (uint32_t w = 0; w < dst_w; w += 2) {
        *(raw + w) = value_u;
        *(raw + w + 1) = value_v;
      }
    } else {
      auto *raw = dst_uv + h * dst_w;
      for (uint32_t w = 0; w < left; w += 2) {
        *(raw + w) = value_u;
        *(raw + w + 1) = value_v;
      }
      raw = dst_uv + h * dst_w + left;
      auto *src_uv_raw = src_uv_data + (h - (top / 2)) * src_w;
      memcpy(raw, src_uv_raw, src_w);
      raw = dst_uv + h * dst_w + left + src_w;
      for (uint32_t w = 0; w < right; w += 2) {
        *(raw + w) = value_u;
        *(raw + w + 1) = value_v;
      }
    }
  }

  return unique;
}

std::unique_ptr<char[]> hobotcv_replicate_padding(const char *src,
                                                  const int &src_h,
                                                  const int &src_w,
                                                  uint32_t top,
                                                  uint32_t bottom,
                                                  uint32_t left,
                                                  uint32_t right) {
  uint32_t dst_w = src_w + left + right;
  uint32_t dst_h = src_h + top + bottom;
  int dst_y_size = dst_w * dst_h;
  size_t dst_size = dst_h * dst_w * 3 / 2;
  std::unique_ptr<char[]> unique(new char[dst_size]);
  auto dst = unique.get();
  char *dst_y = dst;
  char *dst_uv = dst + dst_y_size;
  auto *src_uv = src + src_h * src_w;
  uint32_t dst_bottom_start_line = src_h + top;
  // padding top and bottom
  for (uint32_t h = 0; h < dst_h; ++h) {  // padding y
    auto *raw = dst_y + h * dst_w + left;
    if (h < top) {  // padding top
      memcpy(raw, src, src_w);
    } else if (h >= dst_bottom_start_line) {  // padding bottom
      auto *src_y_raw = src + (src_h - 1) * src_w;
      memcpy(raw, src_y_raw, src_w);
    } else {  // copy src
      auto *src_y_raw = src + (h - top) * src_w;
      memcpy(raw, src_y_raw, src_w);
    }
  }

  for (uint32_t h = 0; h < dst_h / 2; ++h) {  // padding uv
    auto *raw = dst_uv + h * dst_w + left;
    if (h < (top / 2)) {  // top
      memcpy(raw, src_uv, src_w);
    } else if (h >= (dst_bottom_start_line / 2)) {  // bottom
      auto *src_uv_raw = src_uv + ((src_h / 2) - 1) * src_w;
      memcpy(raw, src_uv_raw, src_w);
    } else {  // copy src
      auto *src_uv_raw = src_uv + (h - (top / 2)) * src_w;
      memcpy(raw, src_uv_raw, src_w);
    }
  }

  // padding left and right
  for (uint32_t h = 0; h < dst_h; ++h) {
    // padding left
    auto *dst_left_y = dst_y + h * dst_w;
    auto *dst_left_uv = dst_uv + (h / 2) * dst_w;
    auto *src_left_y = dst_y + h * dst_w + left;
    auto *src_left_uv = dst_uv + (h / 2) * dst_w + left;
    uint16_t *src_left_uv_16 = (uint16_t *)(src_left_uv);
    memset(dst_left_y, *src_left_y, left);
    for (uint32_t w = 0; w < left; w += 2) {
      uint16_t *dst_16 = (uint16_t *)(dst_left_uv + w);
      *dst_16 = *src_left_uv_16;
    }
    // padding right
    auto *dst_right_y = dst_y + h * dst_w + left + src_w;
    auto *dst_right_uv = dst_uv + (h / 2) * dst_w + left + src_w;
    auto *src_right_y = dst_y + h * dst_w + left + src_w - 1;
    auto *src_right_uv = dst_uv + (h / 2) * dst_w + left + src_w - 2;

    uint16_t *src_right_uv_16 = (uint16_t *)(src_right_uv);
    memset(dst_right_y, *src_right_y, right);
    for (uint32_t w = 0; w < right; w += 2) {
      uint16_t *dst_16 = (uint16_t *)(dst_right_uv + w);
      *dst_16 = *src_right_uv_16;
    }
  }

  return unique;
}

std::unique_ptr<char[]> hobotcv_reflect_padding(const char *src,
                                                const int &src_h,
                                                const int &src_w,
                                                uint32_t top,
                                                uint32_t bottom,
                                                uint32_t left,
                                                uint32_t right) {
  uint32_t dst_w = src_w + left + right;
  uint32_t dst_h = src_h + top + bottom;
  int dst_y_size = dst_w * dst_h;
  size_t dst_size = dst_h * dst_w * 3 / 2;
  std::unique_ptr<char[]> unique(new char[dst_size]);
  auto dst = unique.get();
  char *dst_y = dst;
  char *dst_uv = dst + dst_y_size;
  auto *src_uv = src + src_h * src_w;
  uint32_t dst_bottom_start_line = src_h + top;
  // padding top and bottom
  int index_top = top, index_bottom = bottom;
  for (uint32_t h = 0; h < dst_h; ++h) {  // padding y
    auto *raw = dst_y + h * dst_w + left;
    if (h < top) {  // padding top
      auto *src_y_raw = src + index_top * src_w;
      memcpy(raw, src_y_raw, src_w);
      index_top--;
    } else if (h >= dst_bottom_start_line) {  // padding bottom
      auto *src_y_raw = src + (src_h - bottom + index_bottom - 1) * src_w;
      memcpy(raw, src_y_raw, src_w);
      index_bottom--;
    } else {  // copy src
      auto *src_y_raw = src + (h - top) * src_w;
      memcpy(raw, src_y_raw, src_w);
    }
  }

  index_top = top / 2;
  index_bottom = bottom / 2;
  for (uint32_t h = 0; h < dst_h / 2; ++h) {  // padding uv
    auto *raw = dst_uv + h * dst_w + left;
    if (h < (top / 2)) {  // top
      auto *src_uv_raw = src_uv + index_top * src_w;
      memcpy(raw, src_uv_raw, src_w);
      index_top--;
    } else if (h >= (dst_bottom_start_line / 2)) {  // bottom
      auto *src_uv_raw =
          src_uv + ((src_h / 2) - (bottom / 2) + index_bottom - 1) * src_w;
      memcpy(raw, src_uv_raw, src_w);
      index_bottom--;
    } else {  // copy src
      auto *src_uv_raw = src_uv + (h - (top / 2)) * src_w;
      memcpy(raw, src_uv_raw, src_w);
    }
  }

  // padding left and right
  for (uint32_t h = 0; h < dst_h; ++h) {
    // padding left
    auto *dst_left_y = dst_y + h * dst_w;
    auto *dst_left_uv = dst_uv + (h / 2) * dst_w;
    auto *src_left_y = dst_y + h * dst_w + left;
    auto *src_left_uv = dst_uv + (h / 2) * dst_w + left;
    for (uint32_t w = 0; w < left; w++) {
      *(dst_left_y + w) = *(src_left_y + left - w);
      if (w % 2 == 0) {
        uint16_t *dst_16 = (uint16_t *)(dst_left_uv + w);
        uint16_t *src_left_uv_16 = (uint16_t *)(src_left_uv + left - w);
        *dst_16 = *src_left_uv_16;
      }
    }
    // padding right
    auto *dst_right_y = dst_y + h * dst_w + left + src_w;
    auto *dst_right_uv = dst_uv + (h / 2) * dst_w + left + src_w;
    auto *src_right_y = dst_y + h * dst_w + left + src_w - 1;
    auto *src_right_uv = dst_uv + (h / 2) * dst_w + left + src_w - 2;
    for (uint32_t w = 0; w < right; w++) {
      *(dst_right_y + w) = *(src_right_y - w);
      if (w % 2 == 0) {
        uint16_t *dst_16 = (uint16_t *)(dst_right_uv + w);
        uint16_t *src_right_uv_16 = (uint16_t *)(src_right_uv - w);
        *dst_16 = *src_right_uv_16;
      }
    }
  }

  return unique;
}
}  // namespace hobot_cv
